/***
 * 这段代码是一个C++头文件，定义了用于对象预测的几个类，
 * 这些类属于 fast_planner 命名空间。这些类用于在机器人运动规划中预测其他对象（
 * 如人或其他机器人）的未来位置和速度。
 *
 * 这个头文件为机器人运动规划中的动态对象预测提供了一个框架。
 * 通过使用历史数据和多项式拟合，它可以预测对象在未来一段时间内的位置和速度。
 * 这对于机器人在复杂环境中进行安全导航和避障至关重要。
 * 通过预测其他对象的未来行为，机器人可以更好地规划自己的路径，避免碰撞，并有效地达到目的地。***/
#ifndef _OBJ_PREDICTOR_H_
#define _OBJ_PREDICTOR_H_

#include <Eigen/Eigen>
#include <algorithm>
#include <geometry_msgs/PoseStamped.h>
#include <iostream>
#include <list>
#include <ros/ros.h>
#include <visualization_msgs/Marker.h>

using std::cout;
using std::endl;
using std::list;
using std::shared_ptr;
using std::unique_ptr;
using std::vector;

namespace fast_planner {
class PolynomialPrediction;
typedef shared_ptr<vector<PolynomialPrediction>> ObjPrediction;
typedef shared_ptr<vector<Eigen::Vector3d>> ObjScale;

/* ========== prediction polynomial ========== */
// PolynomialPrediction：这个类用于表示一个对象的轨迹，使用一个多项式来预测对象在未来时间点的位置。
// 它包含了三个6维多项式系数（对应于位置的x、y、z分量），以及定义预测时间范围的起始和结束时间（t1 和 t2）。
// 类中提供了两个方法来计算对象在给定时间点的位置：
// evaluate 方法使用完整的五次多项式，
// evaluateConstVel 方法假设对象以恒定速度移动，只使用多项式系数的前两项。
class PolynomialPrediction {
private:
  vector<Eigen::Matrix<double, 6, 1>> polys;
  double t1, t2;  // start / end

public:
  PolynomialPrediction(/* args */) {
  }
  ~PolynomialPrediction() {
  }

  void setPolynomial(vector<Eigen::Matrix<double, 6, 1>>& pls) {
    polys = pls;
  }
  void setTime(double t1, double t2) {
    this->t1 = t1;
    this->t2 = t2;
  }

  bool valid() {
    return polys.size() == 3;
  }

  /* note that t should be in [t1, t2] */
  // evaluate 方法使用完整的五次多项式，
  Eigen::Vector3d evaluate(double t) {
    Eigen::Matrix<double, 6, 1> tv;
    tv << 1.0, pow(t, 1), pow(t, 2), pow(t, 3), pow(t, 4), pow(t, 5);

    Eigen::Vector3d pt;
    pt(0) = tv.dot(polys[0]), pt(1) = tv.dot(polys[1]), pt(2) = tv.dot(polys[2]);

    return pt;
  }

  // evaluateConstVel 方法假设对象以恒定速度移动，只使用多项式系数的前两项。
  Eigen::Vector3d evaluateConstVel(double t) {
    Eigen::Matrix<double, 2, 1> tv;
    tv << 1.0, pow(t, 1);

    Eigen::Vector3d pt;
    pt(0) = tv.dot(polys[0].head(2)), pt(1) = tv.dot(polys[1].head(2)),
    pt(2) = tv.dot(polys[2].head(2));

    return pt;
  }
};

/* ========== subscribe and record object history ========== */
// ObjHistory：这个类负责记录和存储对象的历史位置数据。它通过订阅ROS主题来接收对象的位置消息，
// 并将其存储在一个列表中。类中定义了一个回调函数 poseCallback 来处理接收到的位置消息，
// 并提供了一个 clear 方法来清空历史记录，以及一个 getHistory 方法来获取历史记录。
class ObjHistory {
public:
  static int skip_num_;
  static int queue_size_;
  static ros::Time global_start_time_;

  ObjHistory() {
  }
  ~ObjHistory() {
  }

  void init(int id);

  void poseCallback(const geometry_msgs::PoseStampedConstPtr& msg);

  void clear() {
    history_.clear();
  }

  void getHistory(list<Eigen::Vector4d>& his) {
    his = history_;
  }

private:
  list<Eigen::Vector4d> history_;  // x,y,z;t
  int skip_;
  int obj_idx_;
  Eigen::Vector3d scale_;
};

/* ========== predict future trajectory using history ========== */
// ObjPredictor：这个类是对象预测系统的主要组成部分。
// 它初始化并管理 ObjHistory 对象，订阅对象的位置消息，并定期调用预测算法来生成对象未来轨迹的预测。
// 它还负责计算对象的尺度信息，这对于在规划中考虑对象的尺寸很重要。
// 类中定义了 predictCallback 方法作为ROS定时器的回调，用于触发预测过程。
class ObjPredictor {
private:
  ros::NodeHandle node_handle_;

  int obj_num_;
  double lambda_;
  double predict_rate_;

  vector<ros::Subscriber> pose_subs_;
  ros::Subscriber marker_sub_;
  ros::Timer predict_timer_;
  vector<shared_ptr<ObjHistory>> obj_histories_;

  /* share data with planner */
  ObjPrediction predict_trajs_;
  ObjScale obj_scale_;
  vector<bool> scale_init_;

  void markerCallback(const visualization_msgs::MarkerConstPtr& msg);

  void predictCallback(const ros::TimerEvent& e);
  void predictPolyFit();
  void predictConstVel();

public:
  ObjPredictor(/* args */);
  ObjPredictor(ros::NodeHandle& node);
  ~ObjPredictor();

  void init();

  ObjPrediction getPredictionTraj();
  ObjScale getObjScale();

  typedef shared_ptr<ObjPredictor> Ptr;
};

}  // namespace fast_planner

#endif